#include "hardware_driver/CanServer.hpp"
#include <iostream>

using namespace std;
namespace HardWareDriver
{
bool CanServer::StartServer(std::string dev, bool startRecThread)
{
    if (!can.Open(dev))
        return false;
    can.EnableLoopBack(false);
    exitFlag = false;
    if (startRecThread)
        recThread = thread(&CanServer::Loop, this);
    return true;
}

void CanServer::RegeistCallBack(uint32_t id, std::function<void(const can_frame &)> callback)
{
    callbacks[id] = callback;
}

void CanServer::RemoveCallBack(uint32_t id)
{
    callbacks.erase(id);
}

void CanServer::Step()
{
    auto frame = can.GetFrame();
    auto callback = callbacks.find(frame.can_id);
    if (callback != callbacks.end())
        callback->second(frame);
}

void CanServer::Loop()
{
    // cout << "server start" << endl;
    while (!exitFlag)
    {
        Step();
        // cout << "on server loop" << endl;
    }
    // cout << "server end" << endl;
}

CanServer::~CanServer()
{
    exitFlag = true;
    if (recThread.joinable())
    {
        recThread.join();
    }
    std::cout << "Can Server Quit" << std::endl;
}
} // namespace HardWareDriver